#include "trafficcontroller.h"
#include "world.h"
#include <QDebug>

//////////////////////////////
//       TRAFFIC SIGNAL      //
///////////////////////////////

TrafficSignal::TrafficSignal()
: mCurrentLeftStatePos(-1), mCurrentRightStatePos(-1),
  mCurrentStraightStatePos(-1), mElapsedTimeInLeftState(0),
  mElapsedTimeInStraightState(0), mElapsedTimeInRightState(0)
{
  // Basic light timer.
  //LightTimeSet green = {1, SIGNAL_GREEN},
  //             yellow = {1, SIGNAL_YELLOW},
  //             red = {1, SIGNAL_RED};
  //mStraightLightTimes.push_back(green);
  //mStraightLightTimes.push_back(yellow);
  //mStraightLightTimes.push_back(red);

}

const StreetMap::Road* TrafficSignal::from() const {return mFromRoad;}
const StreetMap::Road* TrafficSignal::to() const {return mToRoad;}
const StreetMap::Intersection *TrafficSignal::intersection() const {return mIntersection;}
int TrafficSignal::lane() const {return mLane;}
QString TrafficSignal::name() const
{
  QString name = "TrafficSignal_";
  name += mIntersection->name();
  return name;
}
void TrafficSignal::setFrom(const StreetMap::Road* r) {mFromRoad = r;}
void TrafficSignal::setTo(const StreetMap::Road* r) {mToRoad = r;}
void TrafficSignal::setIntersection(const StreetMap::Intersection* i) {mIntersection = i;}
int& TrafficSignal::lane() {return mLane;}

const QList<TrafficSignal::LightTimeSet>& TrafficSignal::straightLightTimes() const
{
  return mStraightLightTimes;
}
const QList<TrafficSignal::LightTimeSet>& TrafficSignal::rightLightTimes() const
{
  return mRightLightTimes;
}
const QList<TrafficSignal::LightTimeSet>& TrafficSignal::leftLightTimes() const
{
  return mLeftLightTimes;
}

TrafficSignal::SIGNAL_STATE TrafficSignal::getState(
    TrafficSignal::LIGHT_POSITION lp) const
{
  const QList<LightTimeSet>* lts = NULL;
  int p = -1;
  switch (lp)
  {
    case LIGHT_POS_LEFT: lts = &mLeftLightTimes; p=mCurrentLeftStatePos; break;
    case LIGHT_POS_RIGHT: lts = &mRightLightTimes;; p=mCurrentRightStatePos; break;
    case LIGHT_POS_STRAIGHT: lts = &mStraightLightTimes;; p=mCurrentStraightStatePos; break;
  }
  if (p != -1) return lts->at(p).state;
  else return SIGNAL_NONE;
}

void TrafficSignal::removeLightTime(int pos, TrafficSignal::LIGHT_POSITION lp)
{
  QList<LightTimeSet>* lts = NULL;
  int *p = NULL;
  switch (pos)
  {
    case LIGHT_POS_LEFT: lts = &mLeftLightTimes; p = &mCurrentLeftStatePos; break;
    case LIGHT_POS_RIGHT: lts = &mRightLightTimes; p = &mCurrentRightStatePos; break;
    case LIGHT_POS_STRAIGHT: lts = &mStraightLightTimes; p = &mCurrentStraightStatePos; break;
  }

  if (lts == NULL)
  {
    if (lts->count() > pos && pos >= 0)
    {
      lts->removeAt(pos);
      if (*p == pos && lts->count() <= *p)
      {
        *p = lts->count()-1;
      }
    }
  }
}

void TrafficSignal::addLightTime(
  TrafficSignal::SIGNAL_STATE state,
  qreal timeInSecs,
  TrafficSignal::LIGHT_POSITION lp)
{
  QList<LightTimeSet> *l = NULL;
  int *p = NULL;
  LightTimeSet lts = {timeInSecs, state};
  switch (lp)
  {
    case LIGHT_POS_LEFT: l = &mLeftLightTimes; p = &mCurrentLeftStatePos; break;
    case LIGHT_POS_RIGHT: l = &mRightLightTimes; p = &mCurrentRightStatePos; break;
    case LIGHT_POS_STRAIGHT: l = &mStraightLightTimes; p = &mCurrentStraightStatePos; break;
  }
  l->push_back(lts);
  if (*p == -1)
    *p = 0;
}

void TrafficSignal::addLightTime(
  TrafficSignal::SIGNAL_STATE state,
  qreal timeInSecs, int pos,
  TrafficSignal::LIGHT_POSITION lp)
{
  QList<LightTimeSet> *l = NULL;
  switch (lp)
  {
    case LIGHT_POS_LEFT: l = &mLeftLightTimes; break;
    case LIGHT_POS_RIGHT: l = &mRightLightTimes; break;
    case LIGHT_POS_STRAIGHT: l = &mStraightLightTimes; break;
  }
  if (l->count() > pos && pos >= 0)
  {
    LightTimeSet lts = {timeInSecs, state};
    l->replace(pos, lts);
  }
}

void TrafficSignal::tick()
{
  //qDebug() << "Traffic Signal ticking!";
  //qDebug() << "1";
  //World *w = World::getInstance();
  mElapsedTimeInLeftState += World::gSimTimeStep;
  mElapsedTimeInRightState += World::gSimTimeStep;
  mElapsedTimeInStraightState += World::gSimTimeStep;
  //qDebug() << "2";
  if (mCurrentLeftStatePos != -1 && mLeftLightTimes.at(mCurrentLeftStatePos).time < mElapsedTimeInLeftState)
  {
    //qDebug() << "2.1";
    // Move the signal to the next in step and change the elapsed time
    // it has been in the new state to whatever the carry over is.
    mCurrentLeftStatePos = (mCurrentLeftStatePos+1)%mLeftLightTimes.count();
    //qDebug() << "2.2";
    mElapsedTimeInLeftState = 0;//mElapsedTimeInLeftState%w->getSimSpeed();
    //qDebug() << "2.3";
  }
  //qDebug() << "3";
  if (mCurrentRightStatePos != -1 && mRightLightTimes.at(mCurrentRightStatePos).time < mElapsedTimeInRightState)
  {
    //qDebug() << "3.1";
    // Move the signal to the next in step and change the elapsed time
    // it has been in the new state to whatever the carry over is.
    mCurrentRightStatePos = (mCurrentRightStatePos+1)%mRightLightTimes.count();
    //qDebug() << "3.2";
    mElapsedTimeInRightState = 0;//mElapsedTimeInRightState%w->getSimSpeed();
    //qDebug() << "3.3";
  }
  //qDebug() << "4";
  if (mCurrentStraightStatePos != -1 && mStraightLightTimes.at(mCurrentStraightStatePos).time < mElapsedTimeInStraightState)
  {
    //qDebug() << "4.1";
    // Move the signal to the next in step and change the elapsed time
    // it has been in the new state mCurrentStraightStatePos whatever the carry over is.
    mCurrentStraightStatePos = (mCurrentStraightStatePos+1)%mStraightLightTimes.count();
    //qDebug() << "4.2";
    mElapsedTimeInStraightState = 0;//mElapsedTimeInStraightState%w->getSimSpeed();
    //qDebug() << "4.3";
  }
  //qDebug() << "TrafficSignal @ " << mIntersection->location().toString() << " in lane " << mLane << " from road ("
  //         << mFromRoad->first().toString() << "=>" << mFromRoad->second().toString() << ")";
  //if (mCurrentLeftStatePos != -1)
  //  qDebug() << "LEFT: " << SignalStateToStr(mLeftLightTimes.at(mCurrentLeftStatePos).state);
  //if (mCurrentStraightStatePos != -1)
  //  qDebug() << "STRAIGHT: " << SignalStateToStr(mStraightLightTimes.at(mCurrentStraightStatePos).state);
  //if (mCurrentRightStatePos != -1)
  //  qDebug() << "RIGHT: " << SignalStateToStr(mRightLightTimes.at(mCurrentRightStatePos).state);
}

QString TrafficSignal::SignalStateToStr(TrafficSignal::SIGNAL_STATE state)
{
  switch (state)
  {
    case SIGNAL_RED: return "RED"; break;
    case SIGNAL_YELLOW: return "YELLOW"; break;
    case SIGNAL_GREEN: return "GREEN"; break;
    case SIGNAL_NONE: return "NONE"; break;
  }
  return "NONE";
}
